#include <ros/ros.h> 
#include <visualization_msgs/Marker.h>
#include <stdio.h>
#include <sstream>
#include <cmath>
#include <fstream>

#include "path/pathplanner.h"
#include "path/path.h"

#include "motion/Matrix.h"
#include "motion/Leg.h"
#include "motion/Robot.h"
#include "motion/Link.h"
#include "motion/Gait.h"
#include <cmath>
#include "motion/XYController.h"

// Contants
//#define PI 3.1415926535897932384626433832795
#define ROBOT_WIDTH 100
#define PTS_PER_TEN_CM 4
#define OBSTACLE_GROWTH (ROBOT_WIDTH / 10 / 2) 

using namespace std;

double map3d[111][681];
double map2d[681];
int **occupancy;
int numPoints = 0;

void filterMap(double, int);
void constructOccupancyGrid();
void expandOccupancyGrid();
void plotRobot(ros::Publisher, double, double, double);
void plot3dMap(ros::Publisher);
void plotOccupancyGrid(ros::Publisher);
void plotPath(Path*, ros::Publisher);
void plotPoint(int, int, ros::Publisher);

int main(int argc, char** argv) {
	ros::init(argc, argv, "sample");
	ros::NodeHandle n;
	ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
	occupancy = (int**)malloc(40 * sizeof(int *));

	for(int i=0; i<40; i++) {
		occupancy[i] = (int*) malloc(170 * sizeof(int));
	}

	// Initialize the maps
	for(int i=0; i<681; i++) {
		for(int j=0; j<111; j++) {
			map3d[j][i] = 4.0;
		}
		map2d[i] = 4.0;
	}

	for(int i=0; i<40; i++) {
		for(int j=0; j<170; j++) {
			occupancy[i][j] = 0;
		}
	}

	// Read the CSV data
	FILE *fp = fopen("/home/sabertooth/ROS/sabertooth/resources/dataTransformed.csv", "r");
	if (fp == NULL) {
		cout << "Data file not found." << endl;
		return 1;
	} else {
		char c;
		int i=0, j=0;
		stringstream ss;

		do {
			c = fgetc(fp);

			if(c == ',') {
				char* end;
				double curr = strtod(ss.str().c_str(), &end);

				// Undo previous linearization
				double t = (double)j * 240 / 681 * 3.14159265 / 180;
				curr /= sin(t);
/*
				// Perform new linearization - modification of (maxDistance/2)-(1/r)
				if(curr > 1.3) {
					curr = (2 - 1 / pow(curr, 4));
				} else {
					curr = (2 - 1 / curr);
				}
*/
				map3d[i][j] = curr;

				j++;
				if(j > 680) {
					j=0;
					i++;
				} 
				ss.str("");
			} else if (c != '\n') {
				ss << c;
			}
		} while(i < 111);
	}

	// Filter map
	filterMap(0.1, 2);

	// Occupancy grid
	constructOccupancyGrid();
	plotRobot(marker_pub, 0, 0, 0);
	plotOccupancyGrid(marker_pub);
	//plot3dMap(marker_pub);
	expandOccupancyGrid();
	
	// Reverse occ grid
	int** cp = (int**)malloc(40 * sizeof(int *));
	for(int i=0; i<40; i++) {
		cp[i] = (int*) malloc(170 * sizeof(int));
	}

	for(int i=0; i<40; i++) {
		for(int j=0; j<170; j++) {
			cp[i][j] = occupancy[39-i][j];
		}
	}

	// Path planning
	Point a(85,39), b(85,0);
	PathPlanner *pp = new PathPlanner(cp, 170, 40, a, b);
	pp->astar();
	Path *p = pp->getPath();
 	//printPath(p, false);
	plotPath(p, marker_pub);

	// Motion control
	double legx[4];
	double legy[4];
	double legz[4];
	int gaitnum=1;
	int setspeed=1;

	Robot *ST = new Robot();
	Gait *GC = new Gait(ST, gaitnum,setspeed);
	XYController *XY = new XYController(GC);

	for (int legnum = 0; legnum < LEGS; legnum++) {
		//Link 1
		XY->gait->getRobot()->legs[legnum].links[0].setLength(LINK1LENGTH);
		XY->gait->getRobot()->legs[legnum].links[0].setMass(LINK1MASS);
		Matrix CG_Link1(1, 4, 0.076, 90.0, 0.0, 90.0);
		XY->gait->getRobot()->legs[legnum].links[0].setCG(CG_Link1);
		Matrix DH_Link1(1, 4, 0.15, 90.0, 0.0, 90.0);
		XY->gait->getRobot()->legs[legnum].links[0].setDH(DH_Link1);

		//Link 2
		XY->gait->getRobot()->legs[legnum].links[1].setLength(LINK2LENGTH);
		XY->gait->getRobot()->legs[legnum].links[1].setMass(LINK2MASS);
		Matrix CG_Link2(1, 4, 0.115, 0.0, 0.0, 0.0);
		XY->gait->getRobot()->legs[legnum].links[1].setCG(CG_Link2);
		Matrix DH_Link2(1, 4, 0.381, 0.0, 0.0, 0.0);
		XY->gait->getRobot()->legs[legnum].links[1].setDH(DH_Link2);

		//Link 3
		XY->gait->getRobot()->legs[legnum].links[2].setLength(LINK3LENGTH);
		XY->gait->getRobot()->legs[legnum].links[2].setMass(LINK3MASS);
		Matrix CG_Link3(1, 4, 0.128, 0.0, 0.0, 0.0);
		XY->gait->getRobot()->legs[legnum].links[2].setCG(CG_Link3);
		Matrix DH_Link3(1, 4, 0.381, 0.0, 0.0, 0.0);
		XY->gait->getRobot()->legs[legnum].links[2].setDH(DH_Link3);

		//Front legs are 0,1
		if((legnum==0)||(legnum==1)){
			XY->gait->getRobot()->legs[legnum].setUnit(1.0);
		}
		//Back legs are 2,3
		else if((legnum==2)||(legnum==3)){
			XY->gait->getRobot()->legs[legnum].setUnit(-1.0);
		}

		//Set Initial XYZ of leg's tips
		XY->gait->getRobot()->legs[legnum].setDesiredx(0);
		double git = XY->gait->getRobot()->legs[legnum].getUnit()*-0.5*XY->gait->getLength();
		XY->gait->getRobot()->legs[legnum].setDesiredy(git);
		XY->gait->getRobot()->legs[legnum].setDesiredz(XY->gait->getLegmax());
	}
	
	double oldX = 0.0;
	double oldY = 0.0;
	
	// First point is (0, 0), so skip it
	if(p != NULL) {
		p = p->next;
	}

	int pnum = 0;
	while(p != NULL) {
		plotPoint(p->p.x, p->p.y, marker_pub);

		double Xpos = (double)(p->p.x - 85) / 10;
		double Ypos = (double)(39 - p->p.y) / 10;
		XY->setDesiredX(Xpos);
		XY->setDesiredY(Ypos);
		XY->setCurrentX(oldX);
		XY->setCurrentY(oldY);
		XY->calculateIterations();

		cout << (pnum++) << ": (" << Xpos << ", " << Ypos << ")" << endl;

		//GO TO X POSITION
		for(int i=0; i<XY->getI(); i++){
			//cout << "I: " << i << endl;
			if(Xpos>0){

				XY->gait->rbe->setOrientation(4);
			}
			else{
				XY->gait->rbe->setOrientation(3);
			}
			XY->calculateVectors();
			//Go through the gait.
			for(int o=0; o<LEGS; o++){
				legx[o]=XY->gait->getRobot()->legs[o].getDesiredx();
				legy[o]=XY->gait->getRobot()->legs[o].getDesiredy();
				legz[o]=XY->gait->getRobot()->legs[o].getDesiredz();
			}

			for(int g=1; g<XY->gait->getOffset4(); g++){
				XY->gait->setGaitmotion(g); // Set the counter in the Gait

				XY->gait->calculateGait(); // Calculate where the robot should move through the gait

				//Calculate the angles necessary for the robot needs to move to!
				XY->gait->getRobot()->inverseK();

				if(g>3){
					XY->gait->logDesiredPositions();
					XY->gait->getRobot()->logIK();
				}
			}
			for(int o=0; o<LEGS; o++){
				XY->gait->getRobot()->legs[o].setDesiredx(legx[o]);
				XY->gait->getRobot()->legs[o].setDesiredy(legy[o]);
				XY->gait->getRobot()->legs[o].setDesiredz(legz[o]);
			}
		}

		oldX = Xpos;
		oldY = Ypos;
		p = p->next;
	}
/*
	for(int i=0; i<40; i++) {
		for(int j=0; j<170; j++) {
			if(cp[i][j] != 0) {
				cout << cp[i][j];
			}
			cout << ",";
		}
		cout << endl;
	}
*/
	delete XY;
	delete GC;
	delete ST;
	delete p;

	return 0;
}

void filterMap(double tolerance, int filterSize) {
	// Apply minimum range filter
	for(int i=0; i<111; i++) {
		for(int j=0; j<681; j++) {
			double curr = map3d[i][j];
			map3d[i][j] = (curr < 0.2 ? 4.0 : curr);
		}
	}

	// Apply noise filter
	for(int i=0; i<111; i++) {
		for(int j=filterSize; j<681-filterSize; j++) {
			double curr = map3d[i][j];
			double currLeft = map3d[i][j-filterSize];
			double currRight = map3d[i][j+filterSize];

			if(curr < 4) {
				if((currLeft > curr-tolerance) && (currRight > curr-tolerance)) {
					map3d[i][j] = currRight;
				} else if((currLeft < curr+tolerance) && (currRight < curr+tolerance)) {
					map3d[i][j] = currRight;
				} else if((currLeft < curr+tolerance) && (currRight > curr-tolerance)) {
					map3d[i][j] = currRight;
				} else if((currLeft > curr-tolerance) && (currRight < curr+tolerance)) {
					map3d[i][j] = currRight;
				}
			}
		}
	}
}

void constructOccupancyGrid() {
	for(int i=0; i<111; i++) {
		int x = 0;
		for(int j=0; j<681; j+=PTS_PER_TEN_CM) {
			for(int k=j; k<j+PTS_PER_TEN_CM; k++) {
				double y = map3d[i][k] * 10;
				if(y < 40) {
					occupancy[(int)y][x] = 2;
				}
			}
			if(++x > 169) {
				break;
			}
		}
	}
}

void expandOccupancyGrid() {
	for(int i=0; i<40; i++) {
		for(int j=0; j<170; j++) {
			if(occupancy[i][j] == 2) {
				int miny = i - OBSTACLE_GROWTH;
				int maxy = i + OBSTACLE_GROWTH;
				int minx = j - OBSTACLE_GROWTH;
				int maxx = j + OBSTACLE_GROWTH;

				while(miny < 0) miny++;
				while(maxy > 39) maxy--;
				while(minx < 0) minx++;
				while(maxx > 169) maxx--;

				for(int y=miny; y<maxy; y++) {
					for(int x=minx; x<maxx; x++) {
						occupancy[y][x] = 1;
					}
				}
			} 
		}
	}
}

void plotRobot(ros::Publisher marker_pub, double x, double y, double z) {
	visualization_msgs::Marker robot;
	robot.header.frame_id = "/lidar_frame";
	robot.header.stamp = ros::Time::now();
	robot.ns = "sample";
	robot.action = visualization_msgs::Marker::ADD;

	// Config the robot
	robot.id = numPoints++;
	robot.type = visualization_msgs::Marker::CUBE;
	robot.pose.position.x = x;
	robot.pose.position.y = y;
	robot.pose.position.z = z;
	robot.scale.x = robot.scale.y = robot.scale.z = 0.2;
	robot.color.r = 1.0f;
	robot.color.a = 1.0f; 

	marker_pub.publish(robot);
}

void plot3dMap(ros::Publisher marker_pub) {
	for (int i=0; i<111; i++) {
		visualization_msgs::Marker points;
		points.header.frame_id = "/lidar_frame";
		points.header.stamp = ros::Time::now();
		points.ns = "sample";
		points.action = visualization_msgs::Marker::ADD;
		points.pose.orientation.w = 1.0;

		// Config the points
		points.id = numPoints++;
		points.type = visualization_msgs::Marker::POINTS;
		points.scale.x = 0.03f;
		points.scale.y = 0.03f;
		points.color.g = 1.0f;
		points.color.a = 1.0f;
    
		// Create the vertices for the points
		for(int j=0; j<681; j++) {
			double x = (double) j - 681.0 / 2.0;
			double y = map3d[i][j];
			double z = (double) i;

			if(y > 0.02 && y < 4) {
				geometry_msgs::Point p;
				p.x = x/100;
				p.y = y;
				p.z = z/50;

				points.points.push_back(p);
			}
		}
		marker_pub.publish(points);
	}
}

void plotOccupancyGrid(ros::Publisher marker_pub) {
	for (int i=0; i<40; i++) {
		visualization_msgs::Marker points;
		points.header.frame_id = "/lidar_frame";

		points.header.stamp = ros::Time::now();
		points.ns = "sample";
		points.action = visualization_msgs::Marker::ADD;
		points.pose.orientation.w = 1.0;

		// Config the points
		points.id = numPoints++;
		points.type = visualization_msgs::Marker::POINTS;
		points.scale.x = 0.03f;
		points.scale.y = 0.03f;
		points.color.g = 1.0f;
		points.color.a = 1.0f;

		// Create the vertices for the points
		for(int j=0; j<170; j++) {
			double x = (double) j - 170.0 / 2.0;
			double y = (double) i;
			double z = occupancy[i][j];

			if(z > 0) {
				geometry_msgs::Point p;
				p.x = x / 25;
				p.y = y / 10;
				p.z = z;

				points.points.push_back(p);
			}
		}
		marker_pub.publish(points);

	}
}

void plotPath(Path* path, ros::Publisher marker_pub) {
	while(path != NULL) {
		visualization_msgs::Marker points;
		points.header.frame_id = "/lidar_frame";
		points.header.stamp = ros::Time::now();
		points.ns = "sample";
		points.action = visualization_msgs::Marker::ADD;
		points.pose.orientation.w = 1.0;

		// Config the points
		points.id = numPoints++;
		points.type = visualization_msgs::Marker::POINTS;
		points.scale.x = 0.03f;
		points.scale.y = 0.03f;
		points.color.b = 1.0f;
		points.color.a = 1.0f;

		// Create the vertices for the points
		double x = (double) path->p.x - 170.0 / 2.0;
		double y = 39.0 - (double) path->p.y;

		geometry_msgs::Point p;
		p.x = x / 25;
		p.y = y / 10;
		p.z = 0;

		points.points.push_back(p);
		
		marker_pub.publish(points);
		path = path->next;
	}
}

void plotPoint(int x, int y, ros::Publisher marker_pub) {
	visualization_msgs::Marker points;
	points.header.frame_id = "/lidar_frame";
	points.header.stamp = ros::Time::now();
	points.ns = "sample";
	points.action = visualization_msgs::Marker::ADD;
	points.pose.orientation.w = 1.0;

	// Config the points
	points.id = numPoints++;
	points.type = visualization_msgs::Marker::POINTS;
	points.scale.x = 0.03f;
	points.scale.y = 0.03f;
	points.color.r = 1.0f;
	points.color.a = 1.0f;

	// Create the vertices for the points
	double xx = (double) x - 170.0 / 2.0;
	double yy = 39.0 - (double) y;

	geometry_msgs::Point p;
	p.x = xx / 25;
	p.y = yy / 10;
	p.z = 1;

	points.points.push_back(p);
	marker_pub.publish(points);
}
